/*
 * Copyright (C) 2022 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */
#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"

#define GPIO0 0
#define GPIO1 1
#define GPIO2 2
#define GPIO6 6
#define GPIO7 7
#define GPIO8 8
#define GPIO9 9
#define GPIO10 10
#define GPIO11 11
#define COUNT 10
#define GPIOFUNC 0
#define PWM_FREQ_FREQUENCY  (60000)

void gpio_control (unsigned int  gpio, IotGpioValue value)
{
    hi_io_set_func(gpio, GPIOFUNC);
    IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(gpio, value);
}
// void car_forward1(void)//向前走
// {
//     // unsigned int timeq=30;
//     // while(1){
//     //     gpio_control(GPIO6, IOT_GPIO_VALUE0);//6,7控制一个电机，9，10控制一个电机
//     //     gpio_control(GPIO7, IOT_GPIO_VALUE1);
//     //     hi_sleep(timeq);
//     //     gpio_control(GPIO6, IOT_GPIO_VALUE0);//6,7控制一个电机，9，10控制一个电机
//     //     gpio_control(GPIO7, IOT_GPIO_VALUE0);
//     //     hi_sleep(timeq);
        
//     // }
//     gpio_control(GPIO9, IOT_GPIO_VALUE0);
//     gpio_control(GPIO10, IOT_GPIO_VALUE1);
// }
void car_forward(void)//向前走
{
    gpio_control(GPIO6, IOT_GPIO_VALUE0);//6,7控制一个电机，9，10控制一个电机
    gpio_control(GPIO7, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}

void car_backward(void)
{
    gpio_control(GPIO6, IOT_GPIO_VALUE1);
    gpio_control(GPIO7, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

void car_left(void)
{
    gpio_control(GPIO6, IOT_GPIO_VALUE0);
    gpio_control(GPIO7, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}

void car_right(void)
{
    gpio_control(GPIO6, IOT_GPIO_VALUE0);
    gpio_control(GPIO7, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

void car_stop(void)
{
    gpio_control(GPIO6, IOT_GPIO_VALUE0);
    gpio_control(GPIO7, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}


//原函数控制LED1，原函数+1控制爪子
void set_angle(unsigned int duty)
{
    // unsigned int time = 20000;  //表达的是烧录以后多少秒后开始运行，与角度无关
    unsigned int time = 10000;
    IoTGpioInit(GPIO2);
    IoTGpioSetDir(GPIO2, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(GPIO2, IOT_GPIO_VALUE1);
    hi_udelay(duty);
    IoTGpioSetOutputVal(GPIO2, IOT_GPIO_VALUE0);
    hi_udelay(time - duty);
}
void set_angle1(unsigned int duty)
{
    // unsigned int time = 20000;  //表达的是烧录以后多少秒后开始运行，与角度无关
    unsigned int time2 = 10000;
    IoTGpioInit(GPIO11);
    IoTGpioSetDir(GPIO11, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(GPIO11, IOT_GPIO_VALUE1);
    hi_udelay(duty);
    IoTGpioSetOutputVal(GPIO11, IOT_GPIO_VALUE0);
    hi_udelay(time2 - duty);
}

/*  Steering gear turn left */  //转向器（转向齿轮）向左转
void engine_turn_left(void)
{
    unsigned int angle =2000;//2500对应130度（130度是最大角度），2000对应70度
    for (int i = 0; i < COUNT; i++) {
        set_angle(angle);
    }
}
void engine_turn_left1(void)
{
    unsigned int angle =2530;//爪子对应2530，两个之间距离最小为2.5cm
    for (int i = 0; i < COUNT; i++) {
        set_angle1(angle);
    }
}

/* Steering gear turn right */
void engine_turn_right(void)
{
    // unsigned int angle = 2000;
    unsigned int angle = 700;//1100以下都向右转，到370最大值
    for (int i = 0; i < COUNT; i++) {
        set_angle(angle);
    }
}
void engine_turn_right1(void)
{
    // unsigned int angle = 2000;
    unsigned int angle = 500;//1100以下都向右转，到370最大值
    for (int i = 0; i < COUNT; i++) {
        set_angle1(angle);
    }
}

/* Steering gear return to middle */ //转向齿轮转到中间
void regress_middle(void)
{
    unsigned int angle = 1300;//原为1500，调试1300适合中间位置
    for (int i = 0; i < COUNT; i++) {
        set_angle(angle);
    }
}
void regress_middle1(void)
{
    unsigned int angle = 1500;
    for (int i = 0; i < COUNT; i++) {
        set_angle1(angle);
    }
}
void testwhereturn(void)
{
    unsigned int time = 600;
    unsigned int time1 =2000;
    unsigned int time2 =7000;
    car_forward();
    hi_sleep(time2);
    car_backward();
    hi_sleep(time2);
    car_left();
    hi_sleep(time2);
    car_right();
    hi_sleep(time1);
    car_stop();

    //操控机械臂开始抓取
    engine_turn_left();
    regress_middle1();
    hi_sleep(time);
    engine_turn_left1();
    hi_sleep(time);
    regress_middle();
    //夹起东西后回到中间位置，接下来将其放到篮子中
    hi_sleep(time);
    engine_turn_right();
    hi_sleep(time);
    regress_middle1();
    hi_sleep(time);
    regress_middle();
}
APP_FEATURE_INIT(testwhereturn);
